#include "motion_control.hpp"

#define FREQ 50
#define MIN_US 500
#define MAX_US 2500
#define SERVO_PIN_PWM_1 13
#define SERVO_PIN_PWM_2 12
#define SERVO_PIN_PWM_3 14
#define SERVO_PIN_PWM_4 27

Servo servo_1,servo_2,servo_3,servo_4;
Joint joint;

/**
 * @brief 舵机初始化
 */
void initServo()
{
    ESP32PWM::allocateTimer(0);   // allocate the timer 0 for PWM
    servo_1.setPeriodHertz(FREQ); // Standard 50hz servo
    servo_2.setPeriodHertz(FREQ); // Standard 50hz servo
    servo_3.setPeriodHertz(FREQ); // Standard 50hz servo
    servo_4.setPeriodHertz(FREQ); // Standard 50hz servo

    servo_1.attach(SERVO_PIN_PWM_1, MIN_US, MAX_US); // attaches the servo pin to the servo object
    servo_2.attach(SERVO_PIN_PWM_2, MIN_US, MAX_US); // attaches the servo pin to the servo object
    servo_3.attach(SERVO_PIN_PWM_3, MIN_US, MAX_US); // attaches the servo pin to the servo object
    servo_4.attach(SERVO_PIN_PWM_4, MIN_US, MAX_US); // attaches the servo pin to the servo object

    Serial.println("舵机初始化完成");
}


/**
 * @brief 设置舵机输出角度
 * @param degree_1 
 * @param degree_2 
 * @param degree_3 
 * @param degree_4 
 */
void setServoDegree(int degree_1, int degree_2, int degree_3, int degree_4)
{
    servo_1.write(degree_1);
    servo_2.write(degree_2);
    servo_3.write(degree_3);
    servo_4.write(degree_4);
}

void moveForward()
{
    joint.left[0] = 0;
    setServoDegree(0, 0, 0, 0);
}

void moveBackward()
{
    setServoDegree(60, 60, 60, 60);
}

void turnLeft()
{
    setServoDegree(120, 120, 120, 120);
}

void turnRight()
{
    setServoDegree(180, 180, 180, 180);
}

void stop()
{
    setServoDegree(90, 90, 90, 90);
}